/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "magnetictrack.h"
#include <QSettings>
#include <QVariant>
#include <QDebug>
#include "vehiclekinematics.h"
#include "axisobject.h"
#include "ultrasonicsensor.h"


#define D_DEBUG_ENABLE 0
#define D_PID_MIN_OUTPUT -100000000
#define D_PID_MAX_OUTPUT 100000000

extern UltrasonicSensor *g_ultrasonicSensor;

MagneticTrack::MagneticTrack(QString fileName, MotionMessage *motionMessageIn)
{
    motionMessage=motionMessageIn;

    initialFlag=false;
    lastSensorInfo=0;
    errorCount=0;
    maxErrorCount=20;
    status=E_TRACK_STATUS_STOPPED;
    idealSpeed=0;
    slowdownSpeedRatio=1.0;
    idealRotationSpeed=0;
    isLineMove=true;
    trackMode=E_TRACK_MODE_NONE;
    stopCondition=E_TRACK_STOP_CONDITION_NONE;
    stopStage=0;
    safeSpeedRatio=1;
    magneticPidParameter_P=0;
    magneticPidParameter_I=0;
    magneticPidParameter_D=0;
    moveX_PidParameter_P=0;
    moveX_PidParameter_I=0;
    moveX_PidParameter_D=0;
    moveY_PidParameter_P=0;
    moveY_PidParameter_I=0;
    moveY_PidParameter_D=0;
    moveR_PidParameter_P=0;
    moveR_PidParameter_I=0;
    moveR_PidParameter_D=0;

    laserSafeRatio=1.0;
    userSpeedRatio=1.0;
    robotId=1;

//    barrierSlowDownDistance=0.3;
//    barrierSlowDownRatio=0.2;
//    barrierStopDistance =0.05;

    if(1==readConfigFile(fileName))
    {
        initialFlag=true;
    }
    initialPidController_maganetic();
    initialPidController_virtualLine_rotate();
    initialPidController_virtualLine_xMove();
    initialPidController_virtualLine_yMove();

    //not used
//    if(vehicletype==E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE)
//    {
//        vehicleKinematic=new VehicleKinematics(vehicletype,wheelDiameter,wheelDistance,wheelDistance_height);
//    }
//    else if(vehicletype==E_VEHICLE_TYPE_MCCRUM_4_WHEEL)
//    {
//        vehicleKinematic=new VehicleKinematics(vehicletype,wheelDiameter,wheelDistance,wheelDistance_height);
//    }

    initialSpeedRatioAxis();
}

int MagneticTrack::getFrontMaganetic_deviceAlias()
{
    return frontMaganetic_deviceAlias;
}

int MagneticTrack::getBackMaganetic_deviceAlias()
{
    return backMaganetic_deviceAlias;
}

int MagneticTrack::getFrontRfid_deviceAlias()
{
    return frontRfid_deviceAlias;
}

int MagneticTrack::getBackRfid_deviceAlias()
{
    return backRfid_deviceAlias;
}


int MagneticTrack::changeToVmarkMode()
{
//    qDebug()<<"MagneticTrack::changeToVmarkMode moveX_PidParameter_P_vmark"<<moveX_PidParameter_P_vmark;
    PIDTuningKpSet(pidController_xMove, moveX_PidParameter_P_vmark);
    PIDTuningKpSet(pidController_yMove, moveY_PidParameter_P_vmark);
    return 1;
}

int MagneticTrack::exitVmarkMode()
{
//    qDebug()<<"MagneticTrack::exitVmarkMode moveX_PidParameter_P"<<moveX_PidParameter_P;
    PIDTuningKpSet(pidController_xMove, moveX_PidParameter_P);
    PIDTuningKpSet(pidController_yMove, moveY_PidParameter_P);
    return 1;
}

int MagneticTrack::setMaganeticMovingSpeedRatio(double speedRatioIn)
{
    return 1;//not used;

    userSpeedRatio=speedRatioIn;
    return 1;
}

int MagneticTrack::getSafeSpeedRatio(double &ratioOut)
{
    ratioOut=safeSpeedRatio;
    return 1;
}

int MagneticTrack::setVehicleDriveDirection(bool isPositiveDirectionIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::isPositiveDirectionIn ="<<isPositiveDirectionIn;
    }
    isPositiveDirection=isPositiveDirectionIn;
    return 1;
}

int MagneticTrack::getTrackStatus(E_TRACK_STATUS &statusOut)
{
    statusOut=status;
    return 1;
}

int MagneticTrack::setTrackMode(E_TRACK_MODE modeIn)
{
//    if(trackMode!=E_TRACK_MODE_VIRTUAL_LINE && modeIn==E_TRACK_MODE_VIRTUAL_LINE )
//    {
//        setVehicleMagneticMovePid_maganetic(moveR_PidParameter_P,moveR_PidParameter_I,moveR_PidParameter_D
//                                  ,virtualLinePathVehicleVelocityConpensation,virtualLinePathLineSpeedAdjustMax
//                                  ,virtualLineAngleSpeedAdjustMax);
//    }
//    else  if(trackMode==E_TRACK_MODE_VIRTUAL_LINE && modeIn!=E_TRACK_MODE_VIRTUAL_LINE )
//    {
//        setVehicleMagneticMovePid_maganetic(magneticPidParameter_P,magneticPidParameter_I,magneticPidParameter_D
//                                  ,magneticVehicleVelocityConpensation,magneticLineSpeedAdjustMax
//                                  ,magneticAngleSpeedAdjustMax);
//    }
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setTrackMode ="<<modeIn;
    }
    trackMode=modeIn;
    return 1;
}

int MagneticTrack::setStopCondition(E_TRACK_STOP_CONDITION conditionIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setStopCondition ="<<conditionIn;
    }
    stopCondition=conditionIn;
    return 1;

}

int MagneticTrack::setMageneticPassRfid(long long passRfidIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setMageneticPassRfid ="<<passRfidIn;
    }
    passRfid=passRfidIn;
    return 1;

}

int MagneticTrack::stop()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::stop";
    }
    status=E_TRACK_STATUS_STOPPED;
    resetAll();
    return 1;
}

int MagneticTrack::cease()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::cease";
    }
    status=E_TRACK_STATUS_CEASED;
}

int MagneticTrack::resetAll()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::resetAll";
    }
    lastSensorInfo=0;
    errorCount=0;
    status=E_TRACK_STATUS_STOPPED;
    idealSpeed=0;
    slowdownSpeedRatio=1.0;
    idealRotationSpeed=0;
    isLineMove=true;
    trackMode=E_TRACK_MODE_NONE;
    stopCondition=E_TRACK_STOP_CONDITION_NONE;
    stopStage=0;
    safeSpeedRatio=1;
    resetSpeedRatioAxis();
    PIDReset(pidController_maganetic);
    PIDReset(pidController_rotateMove);
    PIDReset(pidController_xMove);
    PIDReset(pidController_yMove);

    return 1;
}

int MagneticTrack::startRun()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::startRun";
    }
    resetSpeedRatioAxis();
    setSpeedRatioAxisScale(1.0,3);
    status=E_TRACK_STATUS_RUNNING;
}

int MagneticTrack::setVehicleIdealSpeed_ratio(double speedIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setVehicleIdealSpeed_ratio ="<<speedIn;
    }
    idealSpeed=vehicleMaxVel.x_move*speedIn*userSpeedRatio;
    if(0>speedIn)
    {
        isPositiveDirection=false;
    }
    else
    {
        isPositiveDirection=true;
    }
    return 1;
}



int MagneticTrack::setVehicleIdealSpeed_mPerS(double speedIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setVehicleIdealSpeed_mPerS ="<<speedIn;
    }
    idealSpeed=speedIn*userSpeedRatio;
    if(0>speedIn)
    {
        isPositiveDirection=false;
    }
    else
    {
        isPositiveDirection=true;
    }
    return 1;
}

double MagneticTrack::getVehicleIdealSpeed_mPerS()
{
    return idealSpeed;
}

int MagneticTrack::setVehicleIdealRotation_ratio(double speedIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setVehicleIdealRotation_ratio ="<<speedIn;
    }
    idealRotationSpeed==vehicleMaxVel.z_rotate*speedIn;
    return 1;
}

int MagneticTrack::setStart()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setStart";
    }
     status=E_TRACK_STATUS_RUNNING;
     return 1;
}

int MagneticTrack::setLaserSafeRatio(double ratioIn)
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::setLaserSafeRatio";
    }
    laserSafeRatio=ratioIn;
    return 1;
}

int MagneticTrack::quickStop()
{
    if(D_DEBUG_ENABLE)
    {
        qDebug()<<"MagneticTrack::quickStop";
    }
    resetAll();
    return 1;
}

bool MagneticTrack::isInitialOk()
{
    return initialFlag;
}



int MagneticTrack::readConfigFile(QString fileName)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = fileName;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "ControlInfo";
    settings.beginGroup(strConfigGroup);

    //sensorTotalWidth
    strConfigKey = "sensorTotalWidth";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    sensorTotalWidth= strConfigValue.toDouble();

    //magneticPidParameter_P
    strConfigKey = "magneticPidParameter_P";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    magneticPidParameter_P= strConfigValue.toDouble();

    //magneticPidParameter_I
    strConfigKey = "magneticPidParameter_I";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    magneticPidParameter_I= strConfigValue.toDouble();

    //magneticPidParameter_D
    strConfigKey = "magneticPidParameter_D";
    if(false==settings.contains(strConfigKey))
    {
        return -4;
    }
    strConfigValue = settings.value(strConfigKey);
    magneticPidParameter_D= strConfigValue.toDouble();

    //magneticPidParameter_feedforward
    strConfigKey = "magneticPidParameter_feedforward";
    if(false==settings.contains(strConfigKey))
    {
        return -4;
    }
    strConfigValue = settings.value(strConfigKey);
    magneticPidParameter_feedforward= strConfigValue.toDouble();

    //magneticPidParameter_errorLimitForIntegration
    strConfigKey = "magneticPidParameter_errorLimitForIntegration";
    if(false==settings.contains(strConfigKey))
    {
        return -4;
    }
    strConfigValue = settings.value(strConfigKey);
    magneticPidParameter_errorLimitForIntegration= strConfigValue.toDouble();

    //magneticLineSpeedAdjustMax
    strConfigKey = "magneticLineSpeedAdjustMax";
    if(false==settings.contains(strConfigKey))
    {
        return -5;
    }
    strConfigValue = settings.value(strConfigKey);
    lineSpeedAdjustMax_maganetic= strConfigValue.toDouble();

    //magneticAngleSpeedAdjustMax
    strConfigKey = "magneticAngleSpeedAdjustMax";
    if(false==settings.contains(strConfigKey))
    {
        return -6;
    }
    strConfigValue = settings.value(strConfigKey);
    angleSpeedAdjustMax_maganetic= strConfigValue.toDouble();

    //magneticVehicleVelocityConpensation
    strConfigKey = "magneticVehicleVelocityConpensation";
    if(false==settings.contains(strConfigKey))
    {
        return -7;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleVelocityConpensation_maganetic= strConfigValue.toDouble();



    //sampleTime
    strConfigKey = "sampleTime";
    if(false==settings.contains(strConfigKey))
    {
        return -8;
    }
    strConfigValue = settings.value(strConfigKey);
    sampleTime= strConfigValue.toDouble();

    //frontMaganeticDirection
    strConfigKey = "frontMaganeticDirection";
    if(false==settings.contains(strConfigKey))
    {
        return -9;
    }
    strConfigValue = settings.value(strConfigKey);
    frontMaganeticDirection= strConfigValue.toInt();

    //backMaganeticDirection
    strConfigKey = "backMaganeticDirection";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    backMaganeticDirection= strConfigValue.toInt();

    //rfidSensorCount
    strConfigKey = "rfidSensorCount";
    if(false==settings.contains(strConfigKey))
    {
        return -11;
    }
    strConfigValue = settings.value(strConfigKey);
    rfidSensorCount= strConfigValue.toInt();

    //maganeticSensorCount
    strConfigKey = "maganeticSensorCount";
    if(false==settings.contains(strConfigKey))
    {
        return -12;
    }
    strConfigValue = settings.value(strConfigKey);
    maganeticSensorCount= strConfigValue.toInt();


    //allowedReachOffset_line
    strConfigKey = "allowedReachOffset_line";
    if(false==settings.contains(strConfigKey))
    {
        return -13;
    }
    strConfigValue = settings.value(strConfigKey);
    allowedReachOffset_line= strConfigValue.toDouble();



    //allowedReachOffset_angle
    strConfigKey = "allowedReachOffset_angle";
    if(false==settings.contains(strConfigKey))
    {
        return -14;
    }
    strConfigValue = settings.value(strConfigKey);
    allowedReachOffset_angle= strConfigValue.toDouble();

    //maganeticWidthCount_stop
    strConfigKey = "maganeticWidthCount_stop";
    if(false==settings.contains(strConfigKey))
    {
        return -15;
    }
    strConfigValue = settings.value(strConfigKey);
    maganeticWidthCount_stop= strConfigValue.toInt();

    //slowdownRange_line
    strConfigKey = "slowdownRange_line";
    if(false==settings.contains(strConfigKey))
    {
        return -16;
    }
    strConfigValue = settings.value(strConfigKey);
    slowdownRange_line= strConfigValue.toDouble();

    //slowdownRange_angle
    strConfigKey = "slowdownRange_angle";
    if(false==settings.contains(strConfigKey))
    {
        return -17;
    }
    strConfigValue = settings.value(strConfigKey);
    slowdownRange_angle= strConfigValue.toDouble();

    //maxErrorCount
    strConfigKey = "maxErrorCount";
    if(false==settings.contains(strConfigKey))
    {
        return -18;
    }
    strConfigValue = settings.value(strConfigKey);
    maxErrorCount= strConfigValue.toInt();

    //frontMaganetic_deviceAlias
    strConfigKey = "frontMaganetic_deviceAlias";
    if(false==settings.contains(strConfigKey))
    {
        return -19;
    }
    strConfigValue = settings.value(strConfigKey);
    frontMaganetic_deviceAlias= strConfigValue.toInt();

    //backMaganetic_deviceAlias
    strConfigKey = "backMaganetic_deviceAlias";
    if(false==settings.contains(strConfigKey))
    {
        return -20;
    }
    strConfigValue = settings.value(strConfigKey);
    backMaganetic_deviceAlias= strConfigValue.toInt();

    //frontRfid_deviceAlias
    strConfigKey = "frontRfid_deviceAlias";
    if(false==settings.contains(strConfigKey))
    {
        return -21;
    }
    strConfigValue = settings.value(strConfigKey);
    frontRfid_deviceAlias= strConfigValue.toInt();

    //backRfid_deviceAlias
    strConfigKey = "backRfid_deviceAlias";
    if(false==settings.contains(strConfigKey))
    {
        return -22;
    }
    strConfigValue = settings.value(strConfigKey);
    backRfid_deviceAlias= strConfigValue.toInt();

    //isUseFrontMaganetic
    strConfigKey = "isUseFrontMaganetic";
    if(false==settings.contains(strConfigKey))
    {
        return -22;
    }
    strConfigValue = settings.value(strConfigKey);
    isUseFrontMaganetic= strConfigValue.toBool();

    //isUseBackMaganetic
    strConfigKey = "isUseBackMaganetic";
    if(false==settings.contains(strConfigKey))
    {
        return -22;
    }
    strConfigValue = settings.value(strConfigKey);
    isUseBackMaganetic= strConfigValue.toBool();

    //isUseFrontRfid
    strConfigKey = "isUseFrontRfid";
    if(false==settings.contains(strConfigKey))
    {
        return -22;
    }
    strConfigValue = settings.value(strConfigKey);
    isUseFrontRfid= strConfigValue.toBool();

    //isUseBackRfid
    strConfigKey = "isUseBackRfid";
    if(false==settings.contains(strConfigKey))
    {
        return -22;
    }
    strConfigValue = settings.value(strConfigKey);
    isUseBackRfid= strConfigValue.toBool();

//    //barrierSlowDownDistance
//    strConfigKey = "barrierSlowDownDistance";
//    if(false==settings.contains(strConfigKey))
//    {
//        return -8;
//    }
//    strConfigValue = settings.value(strConfigKey);
//    barrierSlowDownDistance= strConfigValue.toDouble();

//    //barrierSlowDownRatio
//    strConfigKey = "barrierSlowDownRatio";
//    if(false==settings.contains(strConfigKey))
//    {
//        return -8;
//    }
//    strConfigValue = settings.value(strConfigKey);
//    barrierSlowDownRatio= strConfigValue.toDouble();

//    //barrierStopDistance
//    strConfigKey = "barrierStopDistance";
//    if(false==settings.contains(strConfigKey))
//    {
//        return -8;
//    }
//    strConfigValue = settings.value(strConfigKey);
//    barrierStopDistance= strConfigValue.toDouble();



    settings.endGroup();
    //--------------------------------------------------



    strConfigGroup = "MechanicalInfo";
    settings.beginGroup(strConfigGroup);

    //vehicletype
    strConfigKey = "vehicletype";
    if(false==settings.contains(strConfigKey))
    {
        return -51;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicletype= (E_VEHICLE_TYPE)strConfigValue.toInt();


    //wheelDiameter
    strConfigKey = "wheelDiameter";
    if(false==settings.contains(strConfigKey))
    {
        return -52;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDiameter= strConfigValue.toDouble();

    //wheelDistance
    strConfigKey = "wheelDistance";
    if(false==settings.contains(strConfigKey))
    {
        return -53;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDistance= strConfigValue.toDouble();

    //wheelDistance_height
    strConfigKey = "wheelDistance_height";
    if(false==settings.contains(strConfigKey))
    {
        return -53;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDistance_height= strConfigValue.toDouble();

    //wheelMaxSpeed
    strConfigKey = "wheelMaxSpeed";
    if(false==settings.contains(strConfigKey))
    {
        return -54;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelMaxSpeed= strConfigValue.toDouble();

    //maxSpeedX
    strConfigKey = "maxSpeedX";
    if(false==settings.contains(strConfigKey))
    {
        return -55;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.x_move= strConfigValue.toDouble();



    //maxSpeedY
    strConfigKey = "maxSpeedY";
    if(false==settings.contains(strConfigKey))
    {
        return -56;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.y_move= strConfigValue.toDouble();

    //maxSpeedRotation
    strConfigKey = "maxSpeedRotation";
    if(false==settings.contains(strConfigKey))
    {
        return -57;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.z_rotate= strConfigValue.toDouble();

    settings.endGroup();

    //--------------------
    strConfigGroup = "VirtualLineControlInfo";
    settings.beginGroup(strConfigGroup);

    //pidParameter_P
    strConfigKey = "moveX_PidParameter_P";
    if(false==settings.contains(strConfigKey))
    {
        return -81;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_P= strConfigValue.toDouble();

    //pidParameter_P
    strConfigKey = "moveX_PidParameter_P_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -81;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_P_vmark= strConfigValue.toDouble();

    //pidParameter_I
    strConfigKey = "moveX_PidParameter_I";
    if(false==settings.contains(strConfigKey))
    {
        return -82;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_I= strConfigValue.toDouble();

    //pidParameter_D
    strConfigKey = "moveX_PidParameter_D";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_D= strConfigValue.toDouble();

    //moveX_PidParameter_feedforward
    strConfigKey = "moveX_PidParameter_feedforward";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_feedforward= strConfigValue.toDouble();

    //moveX_PidParameter_errorLimitForIntegration
    strConfigKey = "moveX_PidParameter_errorLimitForIntegration";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveX_PidParameter_errorLimitForIntegration= strConfigValue.toDouble();

    //pidParameter_P
    strConfigKey = "moveY_PidParameter_P";
    if(false==settings.contains(strConfigKey))
    {
        return -81;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_P= strConfigValue.toDouble();

    //pidParameter_P
    strConfigKey = "moveY_PidParameter_P_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -81;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_P_vmark= strConfigValue.toDouble();

    //pidParameter_I
    strConfigKey = "moveY_PidParameter_I";
    if(false==settings.contains(strConfigKey))
    {
        return -82;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_I= strConfigValue.toDouble();

    //pidParameter_D
    strConfigKey = "moveY_PidParameter_D";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_D= strConfigValue.toDouble();

    //moveY_PidParameter_feedforward
    strConfigKey = "moveY_PidParameter_feedforward";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_feedforward= strConfigValue.toDouble();

    //moveY_PidParameter_errorLimitForIntegration
    strConfigKey = "moveY_PidParameter_errorLimitForIntegration";
    if(false==settings.contains(strConfigKey))
    {
        return -83;
    }
    strConfigValue = settings.value(strConfigKey);
    moveY_PidParameter_errorLimitForIntegration= strConfigValue.toDouble();

    //virtualLinePathPidParameter_P
    strConfigKey = "moveR_PidParameter_P";
    if(false==settings.contains(strConfigKey))
    {
        return -84;
    }
    strConfigValue = settings.value(strConfigKey);
    moveR_PidParameter_P= strConfigValue.toDouble();

    //virtualLinePathPidParameter_I
    strConfigKey = "moveR_PidParameter_I";
    if(false==settings.contains(strConfigKey))
    {
        return -85;
    }
    strConfigValue = settings.value(strConfigKey);
    moveR_PidParameter_I= strConfigValue.toDouble();

    //virtualLinePathPidParameter_D
    strConfigKey = "moveR_PidParameter_D";
    if(false==settings.contains(strConfigKey))
    {
        return -86;
    }
    strConfigValue = settings.value(strConfigKey);
    moveR_PidParameter_D= strConfigValue.toDouble();

    //moveR_PidParameter_feedforward
    strConfigKey = "moveR_PidParameter_feedforward";
    if(false==settings.contains(strConfigKey))
    {
        return -86;
    }
    strConfigValue = settings.value(strConfigKey);
    moveR_PidParameter_feedforward= strConfigValue.toDouble();

    //moveR_PidParameter_errorLimitForIntegration
    strConfigKey = "moveR_PidParameter_errorLimitForIntegration";
    if(false==settings.contains(strConfigKey))
    {
        return -86;
    }
    strConfigValue = settings.value(strConfigKey);
    moveR_PidParameter_errorLimitForIntegration= strConfigValue.toDouble();

    //virtualLinePathLineSpeedAdjustMax
    strConfigKey = "virtualLinePathLineSpeedAdjustMax";
    if(false==settings.contains(strConfigKey))
    {
        return -87;
    }
    strConfigValue = settings.value(strConfigKey);
    virtualLinePathLineSpeedAdjustMax= strConfigValue.toDouble();

    //virtualLineAngleSpeedAdjustMax
    strConfigKey = "virtualLineAngleSpeedAdjustMax";
    if(false==settings.contains(strConfigKey))
    {
        return -88;
    }
    strConfigValue = settings.value(strConfigKey);
    virtualLineAngleSpeedAdjustMax= strConfigValue.toDouble();

    //virtualLinePathVehicleVelocityConpensation
    strConfigKey = "virtualLinePathVehicleVelocityConpensation";
    if(false==settings.contains(strConfigKey))
    {
        return -89;
    }
    strConfigValue = settings.value(strConfigKey);
    virtualLinePathVehicleVelocityConpensation= strConfigValue.toDouble();


    settings.endGroup();

    qDebug() << "MagneticTrack::readConfigFile sucess";
    return 1;
}

int MagneticTrack::initialPidController_maganetic()
{
    pidController_maganetic=new PIDControl;
    PIDInit(pidController_maganetic, magneticPidParameter_P, magneticPidParameter_I, magneticPidParameter_D,
                        sampleTime, D_PID_MIN_OUTPUT, D_PID_MAX_OUTPUT,
                        AUTOMATIC, DIRECT);
    PIDTuningsErrorLimitForIntegrationSet(pidController_maganetic, magneticPidParameter_errorLimitForIntegration);
    PIDTuningsFeedForwardSet(pidController_maganetic, magneticPidParameter_feedforward);

    PIDSetpointSet(pidController_maganetic,0.0);
    return 1;
}

int MagneticTrack::initialPidController_virtualLine_rotate()
{
    pidController_rotateMove=new PIDControl;
    PIDInit(pidController_rotateMove, moveR_PidParameter_P, moveR_PidParameter_I, moveR_PidParameter_D,
                        sampleTime,  D_PID_MIN_OUTPUT, D_PID_MAX_OUTPUT,
                        AUTOMATIC, DIRECT);

    PIDTuningsErrorLimitForIntegrationSet(pidController_rotateMove, moveR_PidParameter_errorLimitForIntegration);
    PIDTuningsFeedForwardSet(pidController_rotateMove, moveR_PidParameter_feedforward);
    PID_set_feedforward_method(pidController_rotateMove,2);
    PIDSetpointSet(pidController_rotateMove,0.0);

    return 1;

}

int MagneticTrack::initialPidController_virtualLine_xMove()
{
    pidController_xMove=new PIDControl;
    PIDInit(pidController_xMove, moveX_PidParameter_P, moveX_PidParameter_I, moveX_PidParameter_D,
                        sampleTime,  D_PID_MIN_OUTPUT, D_PID_MAX_OUTPUT,
                        AUTOMATIC, DIRECT);
    PIDTuningsErrorLimitForIntegrationSet(pidController_xMove, moveX_PidParameter_errorLimitForIntegration);
    PIDTuningsFeedForwardSet(pidController_xMove, moveX_PidParameter_feedforward);
    PID_set_feedforward_method(pidController_xMove,2);
    PIDSetpointSet(pidController_xMove,0.0);
    return 1;
}

int MagneticTrack::initialPidController_virtualLine_yMove()
{
    pidController_yMove=new PIDControl;
    PIDInit(pidController_yMove, moveY_PidParameter_P, moveY_PidParameter_I, moveY_PidParameter_D,
                        sampleTime,  D_PID_MIN_OUTPUT, D_PID_MAX_OUTPUT,
                        AUTOMATIC, DIRECT);
    PIDTuningsErrorLimitForIntegrationSet(pidController_yMove, moveY_PidParameter_errorLimitForIntegration);
    PIDTuningsFeedForwardSet(pidController_yMove, moveY_PidParameter_feedforward);
    PID_set_feedforward_method(pidController_yMove,2);
    PIDSetpointSet(pidController_yMove,0.0);
    return 1;
}

int MagneticTrack::pidOutput2WheelVel_virtualLine(double pidOut_x, double pidOut_r, QVector<double> &wheelVelOut)
{
    switch (vehicletype)
    {
    case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
    {
        return pidOutput2WheelVel_twoDifferentialDriver_virtualLine(pidOut_x,pidOut_r,wheelVelOut);
    }

        break;
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {

    }

        break;
    case E_VEHICLE_TYPE_TWO_STERRING_WHEEL:
    {

    }

        break;
    default:
    {
        return -1;
    }

    }

    return 1;
}

int MagneticTrack::pidOutput2WheelVel_twoDifferentialDriver_virtualLine(double pidOut_x, double pidOut_r,
                                                                        QVector<double> &wheelVelOut)
{
    QVector<double> tmpLineSpeed;
    tmpLineSpeed.resize(2);
    wheelVelOut.resize(2);
    tmpLineSpeed[0]=pidOut_x+pidOut_r;
    tmpLineSpeed[1]=pidOut_x-pidOut_r;

//    if(tmpLineSpeed[0]>composedLineSpeed+lineSpeedAdjustMax)
//    {
//        tmpLineSpeed[0]=composedLineSpeed+lineSpeedAdjustMax;
//    }
//    else if(tmpLineSpeed[0]<composedLineSpeed-lineSpeedAdjustMax)
//    {
//        tmpLineSpeed[0]=composedLineSpeed-lineSpeedAdjustMax;
//    }

//    if(tmpLineSpeed[1]>composedLineSpeed+lineSpeedAdjustMax)
//    {
//        tmpLineSpeed[1]=composedLineSpeed+lineSpeedAdjustMax;
//    }
//    else if(tmpLineSpeed[1]<composedLineSpeed-lineSpeedAdjustMax)
//    {
//        tmpLineSpeed[1]=composedLineSpeed-lineSpeedAdjustMax;
//    }

    wheelVelOut[0]=360*tmpLineSpeed[0]/(wheelDiameter*M_PI);
    wheelVelOut[1]=360*tmpLineSpeed[1]/(wheelDiameter*M_PI);

    return 1;
}

int MagneticTrack::pidOutput2WheelVel_maganeticTrack(QVector<double> &steeringAngleIn,
                 double pidDriveValue, QVector<double> &wheelVelOut,QVector<double> &steeringAngleOut)
{
    switch (vehicletype)
    {
    case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
    {
        if(true==isLineMove)
        {
            return pidOutput2WheelVel_twoDifferentialDriver_line_maganetic(pidDriveValue,wheelVelOut);
        }
        else
        {
            return pidOutput2WheelVel_twoDifferentialDriver_angle_maganetic(pidDriveValue,wheelVelOut);
        }

    }

        break;
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        steeringAngleOut.resize(1);
        if(true==isLineMove)
        {
            return pidOutput2WheelVel_oneSterringDriver_line_maganetic(steeringAngleIn,pidDriveValue,wheelVelOut,steeringAngleOut[0]);
        }
        else
        {
            return pidOutput2WheelVel_oneSterringDriver_angle_maganetic(pidDriveValue,wheelVelOut);
        }

    }

        break;
    case E_VEHICLE_TYPE_TWO_STERRING_WHEEL:
    {
        if(true==isLineMove)
        {
            return pidOutput2WheelVel_twoSterringDriver_line_maganetic(pidDriveValue,wheelVelOut);
        }
        else
        {
            return pidOutput2WheelVel_twoSterringDriver_angle_maganetic(pidDriveValue,wheelVelOut);
        }

    }

        break;
    default:
    {
        return -1;
    }

    }

    return 1;
}

int MagneticTrack::pidOutput2WheelVel_twoDifferentialDriver_line_maganetic(double pidDriveValue, QVector<double> &wheelVelOut)
{

        QVector<double> tmpLineSpeed;
        tmpLineSpeed.resize(2);
        wheelVelOut.resize(2);
        tmpLineSpeed[0]=composedLineSpeed
                +pidDriveValue*vehicleVelocityConpensation_maganetic*composedLineSpeed;
        tmpLineSpeed[1]=composedLineSpeed
                -pidDriveValue*vehicleVelocityConpensation_maganetic*composedLineSpeed;

        if(tmpLineSpeed[0]>composedLineSpeed+lineSpeedAdjustMax_maganetic)
        {
            tmpLineSpeed[0]=composedLineSpeed+lineSpeedAdjustMax_maganetic;
        }
        else if(tmpLineSpeed[0]<composedLineSpeed-lineSpeedAdjustMax_maganetic)
        {
            tmpLineSpeed[0]=composedLineSpeed-lineSpeedAdjustMax_maganetic;
        }

        if(tmpLineSpeed[1]>composedLineSpeed+lineSpeedAdjustMax_maganetic)
        {
            tmpLineSpeed[1]=composedLineSpeed+lineSpeedAdjustMax_maganetic;
        }
        else if(tmpLineSpeed[1]<composedLineSpeed-lineSpeedAdjustMax_maganetic)
        {
            tmpLineSpeed[1]=composedLineSpeed-lineSpeedAdjustMax_maganetic;
        }

        wheelVelOut[0]=360*tmpLineSpeed[0]/(wheelDiameter*M_PI);
        wheelVelOut[1]=360*tmpLineSpeed[1]/(wheelDiameter*M_PI);

        return 1;
}

int MagneticTrack::pidOutput2WheelVel_twoDifferentialDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut)
{
    double tmpAngleSpeed;
    wheelVelOut.resize(2);
    tmpAngleSpeed=composedRotationSpeed
            +pidDriveValue*vehicleVelocityConpensation_maganetic*composedRotationSpeed;

    if(tmpAngleSpeed>composedRotationSpeed+angleSpeedAdjustMax_maganetic)
    {
        tmpAngleSpeed=composedRotationSpeed+angleSpeedAdjustMax_maganetic;
    }
    else if(tmpAngleSpeed<composedRotationSpeed-angleSpeedAdjustMax_maganetic)
    {
        tmpAngleSpeed=composedRotationSpeed-angleSpeedAdjustMax_maganetic;
    }

    double deltaDistance=(tmpAngleSpeed/180.0)*M_PI*wheelDistance/2;


    wheelVelOut[0]=deltaDistance/(M_PI*wheelDiameter)*360.0;
    wheelVelOut[1]=-wheelVelOut[0];

    return 1;
}

int MagneticTrack::pidOutput2WheelVel_oneSterringDriver_line_maganetic(QVector<double> &steeringAngleIn,
                       double pidDriveValue, QVector<double> &wheelVelOut,double &steeringAngleOut)
{
    if(0==steeringAngleIn.size())
    {
        qDebug()<<"error,0==steeringAngleIn.size()";
        return -1;
    }
    //需要和当前转角关联，转角越大，代表磁条的偏离也越大，输出也越大。
    wheelVelOut.resize(1);
        wheelVelOut[0]=360*composedLineSpeed/(wheelDiameter*M_PI);
    double tmpAngle1=pidDriveValue*vehicleVelocityConpensation_maganetic*composedLineSpeed;
    double tmpAngleSum=tmpAngle1+steeringAngleIn[0];
    steeringAngleOut=tmpAngleSum;
    if(tmpAngleSum>angleSpeedAdjustMax_maganetic)
    {
        steeringAngleOut=angleSpeedAdjustMax_maganetic;
    }
    else if(tmpAngleSum<-angleSpeedAdjustMax_maganetic)
    {
        steeringAngleOut=-angleSpeedAdjustMax_maganetic;
    }

//    qDebug()<<"tmpAngle1"<<tmpAngle1<<"tmpAngleSum"<<tmpAngleSum<<"steeringAngleOut"<<steeringAngleOut
//              <<"pidDriveValue"<<pidDriveValue<<"vehicleVelocityConpensation"<<vehicleVelocityConpensation_maganetic
//                <<"composedLineSpeed"<<composedLineSpeed;

    return 1;
}

int MagneticTrack::pidOutput2WheelVel_oneSterringDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut)
{

}

int MagneticTrack::pidOutput2WheelVel_twoSterringDriver_line_maganetic(double pidDriveValue, QVector<double> &wheelVelOut)
{

}

int MagneticTrack::pidOutput2WheelVel_twoSterringDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut)
{

}

int MagneticTrack::pidCalculateOut(E_TRACK_MODE trackModeIn, QVector<double> &steeringAngleIn,
                                   double x_error, double y_error, double r_error,
                                   double x_feedforward, double y_feedforward, double r_feedforward,
                                   QVector<double> &wheelVelOut,QVector<double> &steeringAngleOut,
                                   double &pidOut_xMove, double &pidOut_yMove, double &pidOut_rotateMove)
{
    int tmpKey;
    if(E_TRACK_MODE_NONE==trackModeIn)
    {
        return 0;
    }
    else if(E_TRACK_MODE_VIRTUAL_LINE==trackModeIn)
    {
        PID_set_feedforward_command(pidController_rotateMove,r_feedforward);
        PID_set_feedforward_command(pidController_xMove,x_feedforward);
        PID_set_feedforward_command(pidController_yMove,y_feedforward);
        PIDInputSet(pidController_rotateMove,r_error);
        PIDInputSet(pidController_xMove,x_error);
        PIDInputSet(pidController_yMove,y_error);
        PIDCompute(pidController_rotateMove);
        PIDCompute(pidController_xMove);
        PIDCompute(pidController_yMove);
        pidOut_xMove=PIDOutputGet(pidController_xMove);
        pidOut_yMove=PIDOutputGet(pidController_yMove);
        pidOut_rotateMove=PIDOutputGet(pidController_rotateMove);
//        pidOut_yMove=0;//暂时不支持全向
//        return pidOutput2WheelVel_twoDifferentialDriver_virtualLine(pidOut_x,pidOut_r,wheelVelOut);
    }
    else if(E_TRACK_MODE_MAGENETIC_MIDDLE==trackModeIn
            || E_TRACK_MODE_MAGENETIC_LEFT==trackModeIn
            ||E_TRACK_MODE_MAGENETIC_RIGHT==trackModeIn)
    {
        PIDInputSet(pidController_maganetic,r_error);
        PIDCompute(pidController_maganetic);
        return pidOutput2WheelVel_maganeticTrack(steeringAngleIn,
                               PIDOutputGet(pidController_maganetic),wheelVelOut,steeringAngleOut);
    }

    return -1;
}

int MagneticTrack::pidInputCalculate_maganetic_middle(QVector<bool> magneticData , double &resultOut)
{
    int tmpLeftIndex=-1;
    int tmpRightIndex=-1;
    int dataSize=magneticData.size();
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[i])
        {
            tmpLeftIndex=i;
            break;
        }
    }
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[dataSize-1-i])
        {
            tmpRightIndex=dataSize-1-i;
            break;
        }
    }

    if(tmpLeftIndex==-1)
    {
        return -1;
    }
    else
    {
        double averageIndex=(tmpLeftIndex+tmpRightIndex)/2;
        resultOut=sensorTotalWidth/dataSize*(averageIndex+1-dataSize/2);
        return 1;
    }
    return 1;
}

int MagneticTrack::pidInputCalculate_maganetic_low(QVector<bool> magneticData, double &resultOut)
{
    int tmpLeftIndex=-1;
    int tmpRightIndex=-1;
    int dataSize=magneticData.size();
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[i])
        {
            tmpLeftIndex=i;
            break;
        }
    }
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[dataSize-1-i])
        {
            tmpRightIndex=dataSize-1-i;
            break;
        }
    }

    if(tmpLeftIndex==-1)
    {
        return -1;
    }
    else
    {
        double averageIndex=tmpLeftIndex;
        resultOut=sensorTotalWidth/dataSize*(averageIndex+1-dataSize/2);
        return 1;
    }
    return 1;
}

int MagneticTrack::pidInputCalculate_maganetic_high(QVector<bool> magneticData, double &resultOut)
{
    int tmpLeftIndex=-1;
    int tmpRightIndex=-1;
    int dataSize=magneticData.size();
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[i])
        {
            tmpLeftIndex=i;
            break;
        }
    }
    for(int i=0;i<dataSize;i++)
    {
        if(true==magneticData[dataSize-1-i])
        {
            tmpRightIndex=dataSize-1-i;
            break;
        }
    }

    if(tmpLeftIndex==-1)
    {
        return -1;
    }
    else
    {
        double averageIndex=tmpRightIndex;
        resultOut=sensorTotalWidth/dataSize*(averageIndex+1-dataSize/2);
        return 1;
    }
    return 1;
}

//int MagneticTrack::pidInputCalculate_virtualLine(bool isPositiveDirectionIn, double absoluteAngle,
//                                                 double deltaAngle, double absoluteDistance, double &resultOut)
//{
//    if(true==isLineMove)
//    {
//        if(allowedReachOffset_line>absoluteDistance )
//        {
//            resultOut=absoluteAngle;
//            return 1;
//        }
//        else
//        {
//            resultOut=deltaAngle;
//            return 1;
//        }
//    }
//    else
//    {
//        resultOut=absoluteAngle;
//        return 1;
//    }
//    return 1;
//}

E_STOP_STATUS MagneticTrack::passConditionLoop(SensorInfo sensorInfoIn)
{
    int tmpKey;
    if(E_TRACK_MODE_NONE==trackMode)
    {
        return E_STOP_STATUS_NORMAL;
    }
    else if(E_TRACK_MODE_VIRTUAL_LINE==trackMode)
    {
        return E_STOP_STATUS_NORMAL;
    }
    else if(E_TRACK_MODE_MAGENETIC_MIDDLE==trackMode)
    {
        if(true==isPositiveDirection)
        {

            if(passRfid==sensorInfoIn.rfidData_front &&
                    E_TRACK_STOP_CONDITION_NONE==stopCondition)
            {
                return E_STOP_STATUS_REACHED;
            }
            else
            {
                return E_STOP_STATUS_NORMAL;
            }
        }
        else
        {
            if(2==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_back &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
            else   if(1==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_front &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
        }

    }
    else if(E_TRACK_MODE_MAGENETIC_LEFT==trackMode)
    {
        if(true==isPositiveDirection)
        {

            if(passRfid==sensorInfoIn.rfidData_front &&
                    E_TRACK_STOP_CONDITION_NONE==stopCondition)
            {
                return E_STOP_STATUS_REACHED;
            }
            else
            {
                return E_STOP_STATUS_NORMAL;
            }
        }
        else
        {
            if(2==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_back &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
            else   if(1==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_front &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
        }

    }
    else if(E_TRACK_MODE_MAGENETIC_RIGHT==trackMode)
    {
        if(true==isPositiveDirection)
        {

            if(passRfid==sensorInfoIn.rfidData_front &&
                    E_TRACK_STOP_CONDITION_NONE==stopCondition)
            {
                return E_STOP_STATUS_REACHED;
            }
            else
            {
                return E_STOP_STATUS_NORMAL;
            }
        }
        else
        {
            if(2==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_back &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
            else   if(1==rfidSensorCount)
            {
                if(passRfid==sensorInfoIn.rfidData_front &&
                        E_TRACK_STOP_CONDITION_NONE==stopCondition)
                {
                    return E_STOP_STATUS_REACHED;
                }
                else
                {
                    return E_STOP_STATUS_NORMAL;
                }
            }
        }

    }
    return E_STOP_STATUS_NORMAL;
}

int MagneticTrack::pidInputCalculate(E_TRACK_MODE trackModeIn, bool isPositiveDirectionIn,
                                     SensorInfo sensorInfoIn, double &resultOut_x, double &resultOut_y,
                                     double& resultOut_r)
{
    int tmpKey;
    if(E_TRACK_MODE_NONE==trackModeIn)
    {
        return 0;
    }
    else if(E_TRACK_MODE_VIRTUAL_LINE==trackModeIn)
    {
        resultOut_x=sensorInfoIn.deltaDistanceX_virtualLine;
        resultOut_y=sensorInfoIn.deltaDistanceY_virtualLine;
        resultOut_r=sensorInfoIn.deltaAngle_virtualLine;
        return 1;
//        return pidInputCalculate_virtualLine(isPositiveDirectionIn,sensorInfoIn.absoluteAngle_virtualLine,
//                         sensorInfoIn.deltaAngle_virtualLine,sensorInfoIn.absoluteDistance_virtualLine,resultOut);
    }
    else if(E_TRACK_MODE_MAGENETIC_MIDDLE==trackModeIn)
    {
        if(true==isPositiveDirectionIn)
        {

            tmpKey=pidInputCalculate_maganetic_middle(sensorInfoIn.magneticData_front,resultOut_r);
            resultOut_r=resultOut_r*frontMaganeticDirection;
            return tmpKey;
        }
        else
        {
            tmpKey=pidInputCalculate_maganetic_middle(sensorInfoIn.magneticData_back,resultOut_r);
            resultOut_r=resultOut_r*backMaganeticDirection;
            return tmpKey;
        }

    }
    else if(E_TRACK_MODE_MAGENETIC_LEFT==trackModeIn)
    {
        if(true==isPositiveDirectionIn)
        {
            tmpKey=pidInputCalculate_maganetic_high(sensorInfoIn.magneticData_front,resultOut_r);
            resultOut_r=resultOut_r*frontMaganeticDirection;
            return tmpKey;
        }
        else
        {
            tmpKey=pidInputCalculate_maganetic_high(sensorInfoIn.magneticData_back,resultOut_r);
            resultOut_r=resultOut_r*backMaganeticDirection;
            return tmpKey;
        }

    }
    else if(E_TRACK_MODE_MAGENETIC_RIGHT==trackModeIn)
    {
        if(true==isPositiveDirectionIn)
        {
            tmpKey=pidInputCalculate_maganetic_low(sensorInfoIn.magneticData_front,resultOut_r);
            resultOut_r=resultOut_r*frontMaganeticDirection;
            return tmpKey;
        }
        else
        {
            tmpKey=pidInputCalculate_maganetic_low(sensorInfoIn.magneticData_back,resultOut_r);
            resultOut_r=resultOut_r*backMaganeticDirection;
            return tmpKey;
        }

    }
    return -1;
}

E_STOP_STATUS MagneticTrack::stopConditionLoop(SensorInfo sensorInfoIn,double &slowdownRatioOut)
{

    switch(stopCondition)
    {
    case E_TRACK_STOP_CONDITION_NONE:
    {
        return E_STOP_STATUS_NORMAL;
    }
        break;
    case E_TRACK_STOP_CONDITION_RFID:
    {
        if(true==isPositiveDirection)
        {
            return stopCondition_rfid(sensorInfoIn.isRfidDetected_front,sensorInfoIn.rfidData_front,slowdownRatioOut);
        }
        else
        {
            if(1==rfidSensorCount)
            {
             return stopCondition_rfid(sensorInfoIn.isRfidDetected_front,sensorInfoIn.rfidData_front,
                                       slowdownRatioOut);
            }
            else if(2==rfidSensorCount)
            {
             return stopCondition_rfid(sensorInfoIn.isRfidDetected_back,sensorInfoIn.rfidData_back,
                                       slowdownRatioOut);
            }
        }

    }
        break;
    case E_TRACK_STOP_CONDITION_MAGANETIC_WIDTH:
    {
        if(true==isPositiveDirection)
        {
            return stopCondition_maganeticWidth(sensorInfoIn.magneticData_front,slowdownRatioOut);
        }
        else
        {
             return stopCondition_maganeticWidth(sensorInfoIn.magneticData_back,slowdownRatioOut);
        }
    }
        break;
    case E_TRACK_STOP_CONDITION_INPUT:
    {

        return stopCondition_input(sensorInfoIn.input,slowdownRatioOut);

    }
        break;
//    case E_TRACK_STOP_CONDITION_VIRTUAL_LINE:
//    {
//        return stopCondition_virtualLine(sensorInfoIn.absoluteDistance_virtualLine,
//                                         sensorInfoIn.absoluteAngle_virtualLine, slowdownRatioOut);
//    }
//        break;
    default:
    {
        return E_STOP_STATUS_ERROR;
    }


    }
    return E_STOP_STATUS_ERROR;

}

E_STOP_STATUS MagneticTrack::stopCondition_input(bool inputDi, double &slowdownRatioOut)
{
    switch(stopStage)
    {
    case 0:
    {
        if(true==inputDi)
        {
            stopStage=1;
            resetSpeedRatioAxis();
            slowdownRatioOut=0;
           return E_STOP_STATUS_REACHED;
        }
        else
        {
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_NORMAL;
        }
    }
        break;
    case 1:
    {
        slowdownRatioOut=0;
       return E_STOP_STATUS_REACHED;

    }
        break;

    }


    return E_STOP_STATUS_ERROR;
}

E_STOP_STATUS MagneticTrack::stopCondition_rfid(bool isRfidDetected,long long rfidData, double &slowdownRatioOut)
{

    switch(stopStage)
    {
    case 0:
    {
        if(true==isRfidDetected && passRfid==rfidData)
        {
            stopStage=1;
            setSpeedRatioAxisScale(0.1,3);
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_SLOWDOWN;
        }
        else
        {
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_NORMAL;
        }
    }
        break;
    case 1:
    {
        if(true==isRfidDetected && passRfid==rfidData)
        {
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_SLOWDOWN;
        }
        else
        {
             stopStage=2;
             resetSpeedRatioAxis();
             slowdownRatioOut=0;
            return E_STOP_STATUS_REACHED;
        }

    }
        break;
    case 2:
    {
        slowdownRatioOut=0;
       return E_STOP_STATUS_REACHED;
    }
        break;
    }



    return E_STOP_STATUS_ERROR;

}

E_STOP_STATUS MagneticTrack::stopCondition_maganeticWidth(QVector<bool> magneticData, double &slowdownRatioOut)
{
    int tmpOnCount=0;
    for(int i=0;i<magneticData.size();i++)
    {
        if(true==magneticData[i])
        {
            tmpOnCount++;
        }
    }


    switch(stopStage)
    {
    case 0:
    {
        if(tmpOnCount>=maganeticWidthCount_stop)
        {
            stopStage=1;
            setSpeedRatioAxisScale(0.1,2);
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_SLOWDOWN;
        }
        else
        {
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_NORMAL;
        }
    }
        break;
    case 1:
    {
        if(tmpOnCount>=maganeticWidthCount_stop)
        {
            speedRatioAxisMoveOneStep();
            slowdownRatioOut=getSpeedRatioAxisScale();
            return E_STOP_STATUS_SLOWDOWN;
        }
        else
        {
             stopStage=2;
             resetSpeedRatioAxis();
             slowdownRatioOut=0;
            return E_STOP_STATUS_REACHED;
        }

    }
        break;
    case 2:
    {
        slowdownRatioOut=0;
       return E_STOP_STATUS_REACHED;
    }
        break;
    }



    return E_STOP_STATUS_ERROR;
}

E_STOP_STATUS MagneticTrack::stopCondition_virtualLine(double deltaDistance, double deltaAngle,
                                                       double &slowdownRatioOut)
{

    if(true==isLineMove)
    {
        switch(stopStage)
        {
        case 0:
        {
            if(slowdownRange_line>=deltaDistance)
            {
                stopStage=1;
//                slowdownRatioOut=deltaDistance/slowdownRange_line;
                return E_STOP_STATUS_SLOWDOWN;
            }
            else
            {
//                speedRatioAxisMoveOneStep();
//                slowdownRatioOut=getSpeedRatioAxisScale();
                return E_STOP_STATUS_NORMAL;
            }
        }
            break;
        case 1:
        {
            if(deltaDistance>=allowedReachOffset_line)
            {
//                slowdownRatioOut=deltaDistance/slowdownRange_line;
                return E_STOP_STATUS_SLOWDOWN;
            }
            else
            {
                 stopStage=2;
//                 slowdownRatioOut=0;
                return E_STOP_STATUS_REACHED;
            }

        }
            break;
        case 2:
        {
//            slowdownRatioOut=0;
            isLineMove=false;
            stopStage=0;
           return E_STOP_STATUS_REACHED;
        }
            break;
        }

    }
    else
    {
        switch(stopStage)
        {
        case 0:
        {
            if(slowdownRange_angle>=deltaAngle)
            {
                stopStage=1;
//                slowdownRatioOut=deltaAngle/slowdownRange_angle;
                return E_STOP_STATUS_SLOWDOWN;
            }
            else
            {
//                speedRatioAxisMoveOneStep();
//                slowdownRatioOut=getSpeedRatioAxisScale();
                return E_STOP_STATUS_NORMAL;
            }
        }
            break;
        case 1:
        {
            if(deltaAngle>=allowedReachOffset_angle)
            {
//                slowdownRatioOut=deltaAngle/slowdownRange_angle;
                return E_STOP_STATUS_SLOWDOWN;
            }
            else
            {
                 stopStage=2;
//                 slowdownRatioOut=0;
                return E_STOP_STATUS_REACHED;
            }

        }
            break;
        case 2:
        {
//            slowdownRatioOut=0;
            isLineMove=false;
           return E_STOP_STATUS_REACHED;
        }
            break;
        }
    }



    return E_STOP_STATUS_ERROR;
}

E_TRACK_LOOP_RESULT MagneticTrack::trackLoop(SensorInfo sensorInfo, QVector<double> &wheelVelOut,QVector<double> &steeringAngleOut,
                                             double &pidOut_xMove, double &pidOut_yMove, double &pidOut_rotateMove)
{
    if(false==initialFlag)
    {
        return E_TRACK_LOOP_RESULT_ERROR;
    }

    if(E_TRACK_STATUS_STOPPED==status || E_TRACK_STATUS_CEASED==status|| E_TRACK_STATUS_ERROR==status)
    {
        if(D_DEBUG_ENABLE)
        {
            qDebug()<<"MagneticTrack::trackLoop return ,status="<<status;
        }
        wheelVelOut.clear();
        return E_TRACK_LOOP_RESULT_ZERO_HALT;
    }
    else if(E_TRACK_STATUS_PASSOVER==status)
    {
        if(D_DEBUG_ENABLE)
        {
            qDebug()<<"MagneticTrack::trackLoop E_TRACK_STATUS_PASSOVER==status "<<status;
        }
        return E_TRACK_LOOP_RESULT_MOVING_HALT;
    }

    if(errorCount>maxErrorCount)
    {
        if(D_DEBUG_ENABLE)
        {
            qDebug()<<"MagneticTrack::trackLoop errorCount>maxErrorCount"<<status;
        }
        wheelVelOut.clear();
        status=E_TRACK_STATUS_ERROR;
        return E_TRACK_LOOP_RESULT_ERROR;
    }

    if(trackMode!=E_TRACK_MODE_VIRTUAL_LINE )
    {
        ultrasonicSensorProtectLoop(isPositiveDirection,ultrasonicSpeedRatio);
        if(ultrasonicSpeedRatio<laserSafeRatio)
        {
            safeSpeedRatio=ultrasonicSpeedRatio;
        }
        else
        {
            safeSpeedRatio=laserSafeRatio;
        }
        checkSensorDevice(sensorInfo);
    }

    //passOver status
    if(E_STOP_STATUS_REACHED==passConditionLoop(sensorInfo))
    {
        status=E_TRACK_STATUS_PASSOVER;
    }
//    //calculate virtual line ideal speed
//    if(trackMode==E_TRACK_MODE_VIRTUAL_LINE )
//    {
//        calculateVirtualLineIdealSpeed(sensorInfo.deltaDistance_virtualLine,idealSpeed);
//    }

    //stop condition loop
    E_STOP_STATUS tmpKey=stopConditionLoop(sensorInfo,slowdownSpeedRatio);
    if(E_STOP_STATUS_NORMAL==tmpKey)
    {
        slowdownSpeedRatio=1.0;
        composedLineSpeed=idealSpeed*slowdownSpeedRatio*safeSpeedRatio;
        composedRotationSpeed=idealRotationSpeed*slowdownSpeedRatio*safeSpeedRatio;
    }
    else if(E_STOP_STATUS_SLOWDOWN==tmpKey)
    {
        composedLineSpeed=idealSpeed*slowdownSpeedRatio*safeSpeedRatio;
        composedRotationSpeed=idealRotationSpeed*slowdownSpeedRatio*safeSpeedRatio;
    }
    else if(E_STOP_STATUS_REACHED==tmpKey)
    {
        slowdownSpeedRatio=1.0;
        wheelVelOut.clear();
        status=E_TRACK_STATUS_GOAL_REACHED;
        return E_TRACK_LOOP_RESULT_ZERO_HALT;
    }
    else if(E_STOP_STATUS_ERROR==tmpKey)
    {
        slowdownSpeedRatio=1.0;
        wheelVelOut.clear();
        status=E_TRACK_STATUS_ERROR;
        return E_TRACK_LOOP_RESULT_ERROR;
    }

//    PIDOutputLimitsSet(pidController,-velAdjust, velAdjust);
//    PIDOutputLimitsSet(pidController,-sensorInfo.resultantIdealSpeed,
//                       sensorInfo.resultantIdealSpeed);
//    PIDOutputLimitsSet(pidController,-velAdjust, +velAdjust);

    double pidOut_x,pidOut_y;
    double pidOut_r;
    if(-1==pidInputCalculate(trackMode,isPositiveDirection,sensorInfo,pidOut_x,pidOut_y,pidOut_r))
    {
       pidOut_r=lastSensorInfo;
       errorCount++;
    }
    else
    {
        errorCount=0;

        //没感应到磁条，就不要输出新速度
        pidCalculateOut(trackMode,sensorInfo.steeringAngle,pidOut_x,pidOut_y,pidOut_r,
                        sensorInfo.forward_x,sensorInfo.forward_y,sensorInfo.forward_r,
                        wheelVelOut,steeringAngleOut,pidOut_xMove,pidOut_yMove,pidOut_rotateMove);

        lastSensorInfo=pidOut_r;
    }



//    qDebug()<<"vehicle pid result, leftVel="<<wheelVelOut<<" rightVel="<<rightVel
//           <<" errorCount="<<errorCount<<"pid_input="<<tmpResult;

    return E_TRACK_LOOP_RESULT_NORMAL;
}

int MagneticTrack::getVirtualLineTrack_feedfowardOutput(double &pidOut_xMove, double &pidOut_yMove, double &pidOut_rotateMove)
{
    if(E_TRACK_MODE_VIRTUAL_LINE==trackMode)
    {
        pidOut_xMove=PIDOutputGet_feedforward(pidController_xMove);
        pidOut_yMove=PIDOutputGet_feedforward(pidController_yMove);
        pidOut_rotateMove=PIDOutputGet_feedforward(pidController_rotateMove);
        return 1;

    }
    else
    {

        return 0;
    }

    return -1;
}

int MagneticTrack::getErrorCode()
{
    return errorCount;
}

//int MagneticTrack::resetError()
//{
//    errorCount=0;
//    lastSensorInfo=0;
//    status=E_TRACK_STATUS_STOPPED;
//    return 1;
//}

int MagneticTrack::setMaxErrorCount(int maxCount)
{
    maxErrorCount=maxCount;
    return 1;
}

int MagneticTrack::setVehicleMagneticMovePid(int idOfPid,TrackPidInfo pidInfo)
{
    if(0==idOfPid)
    {
        qDebug()<<"set pidController_maganetic"<<pidInfo.pid_p<<pidInfo.pid_i<<pidInfo.pid_d
               <<"errorLimitForIntegration"<<pidInfo.errorLimitForIntegration<<"feedforward"<<pidInfo.feedforward;
        magneticPidParameter_P=pidInfo.pid_p;
        magneticPidParameter_I=pidInfo.pid_i;
        magneticPidParameter_D=pidInfo.pid_d;
        magneticPidParameter_feedforward=pidInfo.feedforward;
        magneticPidParameter_errorLimitForIntegration=pidInfo.errorLimitForIntegration;
        PIDTuningsSet(pidController_maganetic,pidInfo.pid_p,pidInfo.pid_i,pidInfo.pid_d);
        PIDTuningsErrorLimitForIntegrationSet(pidController_maganetic, pidInfo.errorLimitForIntegration);
        PIDTuningsFeedForwardSet(pidController_maganetic, pidInfo.feedforward);
        lineSpeedAdjustMax_maganetic=pidInfo.lineAdjustMax;
        angleSpeedAdjustMax_maganetic=pidInfo.rotationAdjustMax;
        vehicleVelocityConpensation_maganetic=pidInfo.pid_velocity_p;
    }
    else if(1==idOfPid)
    {
        qDebug()<<"set pidController_xMove "<<pidInfo.pid_p<<pidInfo.pid_i<<pidInfo.pid_d
               <<"errorLimitForIntegration"<<pidInfo.errorLimitForIntegration<<"feedforward"<<pidInfo.feedforward;
        moveX_PidParameter_P=pidInfo.pid_p;
        moveX_PidParameter_I=pidInfo.pid_i;
        moveX_PidParameter_D=pidInfo.pid_d;
        moveX_PidParameter_feedforward=pidInfo.feedforward;
        moveX_PidParameter_errorLimitForIntegration=pidInfo.errorLimitForIntegration;
        PIDTuningsSet(pidController_xMove,pidInfo.pid_p,pidInfo.pid_i,pidInfo.pid_d);
        PIDTuningsErrorLimitForIntegrationSet(pidController_xMove, pidInfo.errorLimitForIntegration);
        PIDTuningsFeedForwardSet(pidController_xMove, pidInfo.feedforward);

    }
    else if(2==idOfPid)
    {
        qDebug()<<"set pidController_rotateMove"<<pidInfo.pid_p<<pidInfo.pid_i<<pidInfo.pid_d
               <<"errorLimitForIntegration"<<pidInfo.errorLimitForIntegration<<"feedforward"<<pidInfo.feedforward;
        moveR_PidParameter_P=pidInfo.pid_p;
        moveR_PidParameter_I=pidInfo.pid_i;
        moveR_PidParameter_D=pidInfo.pid_d;
        moveR_PidParameter_feedforward=pidInfo.feedforward;
        moveR_PidParameter_errorLimitForIntegration=pidInfo.errorLimitForIntegration;
        PIDTuningsSet(pidController_rotateMove,pidInfo.pid_p,pidInfo.pid_i,pidInfo.pid_d);
        PIDTuningsErrorLimitForIntegrationSet(pidController_rotateMove, pidInfo.errorLimitForIntegration);
        PIDTuningsFeedForwardSet(pidController_rotateMove, pidInfo.feedforward);
    }
    else if(3==idOfPid)
    {
        qDebug()<<"set pidController_yMove"<<pidInfo.pid_p<<pidInfo.pid_i<<pidInfo.pid_d
               <<"errorLimitForIntegration"<<pidInfo.errorLimitForIntegration<<"feedforward"<<pidInfo.feedforward;

        moveY_PidParameter_P=pidInfo.pid_p;
        moveY_PidParameter_I=pidInfo.pid_i;
        moveY_PidParameter_D=pidInfo.pid_d;
        moveY_PidParameter_feedforward=pidInfo.feedforward;
        moveY_PidParameter_errorLimitForIntegration=pidInfo.errorLimitForIntegration;
        PIDTuningsSet(pidController_yMove,pidInfo.pid_p,pidInfo.pid_i,pidInfo.pid_d);
        PIDTuningsErrorLimitForIntegrationSet(pidController_yMove, pidInfo.errorLimitForIntegration);
        PIDTuningsFeedForwardSet(pidController_yMove, pidInfo.feedforward);
    }

    return 1;
}

int MagneticTrack::getPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput)
{
    anglePidOutput=PIDOutputGet(pidController_rotateMove);
    xPidOutput=PIDOutputGet(pidController_xMove);
    yPidOutput=PIDOutputGet(pidController_yMove);
    return 1;
}

int MagneticTrack::checkSensorDevice(const SensorInfo &sensorInfo)
{
    if(isUseFrontRfid && sensorInfo.rfidData_front<0)
    {
        addMsg(ENUM_MSG_ERROR,"MagneticTrack","MagneticTrack",22251,robotId);
    }
    if(isUseBackRfid && sensorInfo.rfidData_back<0)
    {
        addMsg(ENUM_MSG_ERROR,"MagneticTrack","MagneticTrack",22252,robotId);
    }
    if(isUseFrontMaganetic && 0==sensorInfo.magneticData_front.size())
    {
        addMsg(ENUM_MSG_ERROR,"MagneticTrack","MagneticTrack",22253,robotId);
    }
    if(isUseBackMaganetic && 0==sensorInfo.magneticData_back.size())
    {
        addMsg(ENUM_MSG_ERROR,"MagneticTrack","MagneticTrack",22254,robotId);
    }
    return 1;
}


//int MagneticTrack::calculateVirtualLineIdealSpeed(double deltaDistanceIn,double &idealSpeedOut)
//{
//    PIDInputSet(velocityPidController,deltaDistanceIn);
//    PIDCompute(velocityPidController);
//    idealSpeedOut=PIDOutputGet(velocityPidController);

//    if(true==isPositiveDirection && idealSpeedOut<0)
//    {
//        idealSpeedOut=0;
//    }
//    else if(false==isPositiveDirection && idealSpeedOut>0)
//    {
//        idealSpeedOut=0;
//    }
//    return 1;
//}

int MagneticTrack::ultrasonicSensorProtectLoop(bool driveDirection, double &safeSpeedRatioOut)
{
    if(NULL==g_ultrasonicSensor)
    {
        safeSpeedRatioOut=1.0;
        return 0;
    }

    E_BARRIER_TETECT_MODE validModeIn;
    if(true==driveDirection)
    {
        validModeIn=E_BARRIER_TETECT_MODE_FRONT;
    }
    else
    {
        validModeIn=E_BARRIER_TETECT_MODE_BACK;
    }
   return  g_ultrasonicSensor->ultrasonicSensorProtectLoop(validModeIn,safeSpeedRatioOut);

}

int MagneticTrack:: initialSpeedRatioAxis()
{
    //轴初始化
    JointAttribute attribute;
    //trap用
    attribute.maxAccelation=6;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=3;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    //trap用
    attribute.maxAccelation=3;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=10;

    attribute.maxTorque=10000;
    attribute.maxPosition=1.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="speedRatioAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=1;
    attribute.targetFollowError=1;
    attribute.axisLogicalType=E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE;

    speedRatioAxis=new AxisObject(1,attribute,motionMessage);
    speedRatioAxis->axisIdByRobot=9700;
    speedRatioAxis->deltaTime=sampleTime;

    return 1;
}

double MagneticTrack::getSpeedRatioAxisScale()
{
    return speedRatioAxis->positionCommand;
}

int MagneticTrack::setSpeedRatioAxisScale(double ratio, double timeDuration)
{

    speedRatioAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    speedRatioAxis->splineZeroMoveByTime(ratio,timeDuration);

    return 1;
}

int MagneticTrack::speedRatioAxisMoveOneStep()
{
    return speedRatioAxis->moveOneStep();
}

int MagneticTrack::resetSpeedRatioAxis()
{
    speedRatioAxis->resetAll();
    speedRatioAxis->positionCommand=1.0;
    speedRatioAxis->positionCommandOld=1.0;
    speedRatioAxis->positionFeedback=1.0;

}

void MagneticTrack::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn, double parameter1, double parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22254:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotIdIn);
        tmpStr = tmpStr + QObject::tr(", 磁条导航模块获取不到后置磁感应设备信息！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22253:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotIdIn);
        tmpStr = tmpStr + QObject::tr(", 磁条导航模块获取不到前置磁感应设备信息！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22252:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotIdIn);
        tmpStr = tmpStr + QObject::tr(", 磁条导航模块获取不到后置rfid设备信息（信息为负）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22251:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotIdIn);
        tmpStr = tmpStr + QObject::tr(", 磁条导航模块获取不到前置rfid设备信息（信息为负）！");
        infomationStr = tmpStr.toStdString();
        break;
    }


        default:
        {
            qDebug()<<"MagneticTrack::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
